﻿using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using FarseerPhysics.Dynamics;
using FarseerPhysics.Collision.Shapes;
using FarseerPhysics;
using FarseerPhysics.Factories;
using FarseerPhysics.Common;
using Microsoft.Xna.Framework.Input.Touch;
using FarseerPhysics.Dynamics.Joints;
using System.Diagnostics;
using GameName1.Programming;
using System.ComponentModel;
using System.Runtime.CompilerServices;
using System.Reflection;

namespace GameName1
{
    class RobotControl : INotifyPropertyChanged
    {
        public event PropertyChangedEventHandler PropertyChanged;

        private void NotifyPropertyChanged(string propertyName)
        {
            if (PropertyChanged != null)
            {
                PropertyChanged(this, new PropertyChangedEventArgs(propertyName));
            }
        }

        protected bool SetField<T>(ref T field, T value, [CallerMemberName] String propertyName = "")
        {
            if (EqualityComparer<T>.Default.Equals(field, value)) return false;
            field = value;
            NotifyPropertyChanged(propertyName);
            return true;
        }

        private float _leftMotorSpeed;
        public float LeftMotorSpeed
        {
            get { return _leftMotorSpeed; }
            set { SetField(ref _leftMotorSpeed, value); }
        }

        private float _rightMotorSpeed;
        public float RightMotorSpeed
        {
            get { return _rightMotorSpeed; }
            set { SetField(ref _rightMotorSpeed, value); }
        }
    }
    class Robot: GameComponent
    {

        //Size constants (simulation units - m)
        //private static Vector2 ROBOT_SIZE = new Vector2(10, 10);
        //private static Vector2 WHEEL_SIZE = new Vector2(2, 4);

        //private static float C = ROBOT_SIZE.X / 2 - WHEEL_SIZE.X / 2;
        //private static float D = ROBOT_SIZE.Y - WHEEL_SIZE.Y;//distance between wheels
        //private static float E = D / 2;
        const float MOTOR_POWER = 0.1f;
        const float MOTOR_DAMPING = 10f;
        //Animations
        private Texture2D _chassisTexture;
        private Texture2D _wheelTexture;
        private Texture2D _pixelTexture;
        

        //Component positions
        //private Vector2 _leftWheelCenter = new Vector2(WHEEL_SIZE.X / 2, WHEEL_SIZE.Y / 2);
        //private Vector2 _rightWheelCenter = new Vector2(WHEEL_SIZE.X / 2, ROBOT_SIZE.Y - WHEEL_SIZE.Y / 2);
        //private Vector2 _robotCenter = new Vector2(ROBOT_SIZE.X / 2, ROBOT_SIZE.Y / 2);
        private Vector2 _leftWheelPosition;
        private Vector2 _rightWheelPosition;

        //farseer
        private Chassis _chassis;
        private Body _leftWheel;
        private Body _rightWheel;

        private Vector2 _chassisOrigin;
        private Vector2 _wheelOrigin;

        private string _name;
        private string _code;

        private float _leftMotorSpeed, _leftMotorSpeedSet;
        private float _rightMotorSpeed, _rightMotorSpeedSet;

        #region publicproperties
        public float LeftMotorSpeed
        {
            get { return _leftMotorSpeedSet; }
            set { _leftMotorSpeedSet = value; }
        }

        public float RightMotorSpeed
        {
            get { return _rightMotorSpeedSet; }
            set { _rightMotorSpeedSet = value; }
        }

        public string Name
        {
            get { return _name; }
            set { _name = value; }
        }

        public string Code
        {
            get { return _code; }
            set { _code = value; }
        }

        public RobotControl Control;
        #endregion
        private Game _game;
        private float _chassisDensity = 1;
        private float _wheelDensity = 1;

        private Vector2 _chassisPixelSize;
        private Vector2 _chassisSize;

        private Vector2 _wheelPixelSize;
        private Vector2 _wheelSize;

        private List<LineSensor> _lineSensors = new List<LineSensor>();

        private Arena _arena;




        private Interpreter _interpreter;

        
        public Robot(Game game, World world, string textureName, Vector2 initialPositionSim)
            :base(game, initialPositionSim, world)
        {
            _game = game;
            _chassisTexture = _game.Content.Load<Texture2D>(textureName);
            
            _wheelTexture = _game.Content.Load<Texture2D>("wheel1");
            _pixelTexture = _game.Content.Load<Texture2D>("1x1texture");
            
            _chassisPixelSize = new Vector2(_chassisTexture.Width, _chassisTexture.Height);
            _wheelPixelSize = new Vector2(_wheelTexture.Width, _wheelTexture.Height);
            
            _chassisOrigin = _chassisPixelSize / 2f;
            
            _wheelOrigin = _wheelPixelSize / 2f;

            _chassisSize = ConvertUnits.ToSimUnits(_chassisPixelSize);
            _wheelSize = ConvertUnits.ToSimUnits(_wheelPixelSize);
            

            _leftWheelPosition = new Vector2(-_chassisSize.X / 2f - _wheelSize.X / 2f, _chassisSize.Y/2f);
            _rightWheelPosition = new Vector2(_chassisSize.X / 2f + _wheelSize.X / 2f, _chassisSize.Y/2f);

            Vector2 chassisPosition = initialPositionSim;
            
            _chassis = new Chassis(world);
            _chassis.BodyType = BodyType.Dynamic;
            
            _chassis.Position = chassisPosition;
            PolygonShape box = new PolygonShape(PolygonTools.CreateRectangle(_chassisSize.X/2, _chassisSize.Y/2), _chassisDensity);
            _chassis.CreateFixture(box);
            _chassis.LinearDamping = 0.5f;
            _chassis.AngularDamping = 0.5f;

            

            _leftWheel = new Body(world);
            _leftWheel.BodyType = BodyType.Dynamic;
            _leftWheel.Position = chassisPosition + _leftWheelPosition;
            _leftWheel.LinearDamping = 1;
            _leftWheel.AngularDamping = 10000;
            PolygonShape leftWheelShape = new PolygonShape(PolygonTools.CreateRectangle(_wheelSize.X/2f, _wheelSize.Y/2f), _wheelDensity);
            _leftWheel.CreateFixture(leftWheelShape);

            _rightWheel = new Body(world);
            _rightWheel.BodyType = BodyType.Dynamic;
            _rightWheel.Position = chassisPosition + _rightWheelPosition;
            _rightWheel.LinearDamping = 1;
            _rightWheel.AngularDamping = 10000;
            PolygonShape rightWheelShape = new PolygonShape(PolygonTools.CreateRectangle(_wheelSize.X/2f, _wheelSize.Y/2f), _wheelDensity);
            _rightWheel.CreateFixture(rightWheelShape);

            PrismaticJoint leftJoint = new PrismaticJoint(_chassis, _leftWheel, _leftWheelPosition, Vector2.Zero, new Vector2(1, 0));
            leftJoint.LimitEnabled = true;
            leftJoint.LowerLimit = leftJoint.UpperLimit = 0;
            world.AddJoint(leftJoint);
            PrismaticJoint rightJoint = new PrismaticJoint(_chassis, _rightWheel, _rightWheelPosition, Vector2.Zero, new Vector2(1, 0));
            rightJoint.LimitEnabled = true;
            rightJoint.LowerLimit = rightJoint.UpperLimit = 0;
            world.AddJoint(rightJoint);

            _lineSensors.Add(new LineSensor(this, new Vector2(0f, 0f)));

            Control = new RobotControl();
            Control.PropertyChanged += new PropertyChangedEventHandler(control_PropertyChanged);
            _interpreter = new Interpreter(Control);
            
           // _t = new Texture2D(GraphicsDevice, 1, 1);
           // _t.SetData<Color>(
           //     new Color[] { Color.Black });// fill the texture with white
        }

        private void control_PropertyChanged(object sender, PropertyChangedEventArgs e)
        {
            string propertyName = e.PropertyName;
            RobotControl control = sender as RobotControl;
            object val = control.GetType().GetProperty(propertyName).GetValue(control, null);
            Debug.WriteLine("Robot {0}: {1}", propertyName, val);
            PropertyInfo propertyInfo = this.GetType().GetProperty(propertyName);
            propertyInfo.SetValue(this, val);
        }

        internal void TurnOn()
        {
            _interpreter.Run(_code);
        }

        public Arena Arena
        {
            get
            {
                return _arena;
            }
            set
            {
                _arena = value;
            }
        }

        public void Update(GameTime gameTime)
        {
            //Interpreter.Instance.Control(this, gameTime);
            /*TouchCollection touches = TouchPanel.GetState();
            Vector2 leftWheelPosition = ConvertUnits.ToDisplayUnits(_leftWheel.Position);
            Vector2 chassisPosition = ConvertUnits.ToDisplayUnits(_chassis.Position);
            if (touches.Count == 1)
            {
                foreach (TouchLocation tl in touches)
                {
                    //Vector2 forcePixel = tl.Position - leftWheelPosition;
                   Vector2 forcePixel = tl.Position - chassisPosition;
                    Vector2 force = ConvertUnits.ToSimUnits(forcePixel);

                    //DrawLine(batch, tl.Position, leftWheelPosition, Color.Black, 1);
                    _chassis.ApplyForce(force * 8, _chassis.Position);
                    //_leftWheel.ApplyForce(force * 8, _leftWheel.Position);
                }



            }*/
            //driving
            if (_leftMotorSpeedSet <= 0)
            {
                if (_leftMotorSpeed > _leftMotorSpeedSet)
                {
                    _leftMotorSpeed -= MOTOR_POWER;
                }
                if (_leftMotorSpeed < _leftMotorSpeedSet)
                {
                    _leftMotorSpeed = _leftMotorSpeedSet;
                }
            }
            if (_leftMotorSpeedSet >= 0)
            {
                if (_leftMotorSpeed < _leftMotorSpeedSet)
                {
                    _leftMotorSpeed += MOTOR_POWER;
                }
                if (_leftMotorSpeed > _leftMotorSpeedSet)
                {
                    _leftMotorSpeed = _leftMotorSpeedSet;
                }
            }

            if (_rightMotorSpeedSet <= 0)
            {
                if (_rightMotorSpeed > _rightMotorSpeedSet)
                {
                    _rightMotorSpeed -= MOTOR_POWER;
                }
                if (_rightMotorSpeed < _rightMotorSpeedSet)
                {
                    _rightMotorSpeed = _rightMotorSpeedSet;
                }
            }

            if (_rightMotorSpeedSet >= 0)
            {
                if (_rightMotorSpeed < _rightMotorSpeedSet)
                {
                    _rightMotorSpeed += MOTOR_POWER;
                }
                if (_rightMotorSpeed > _rightMotorSpeedSet)
                {
                    _rightMotorSpeed = _rightMotorSpeedSet;
                }
            }

            _leftWheel.LinearDamping = MOTOR_DAMPING / (Math.Abs(_leftMotorSpeedSet)+1);
            _rightWheel.LinearDamping = MOTOR_DAMPING / (Math.Abs(_rightMotorSpeedSet) + 1);


            //setLinearVelocity(_leftWheel,_leftMotorSpeed);
           // setLinearVelocity(_rightWheel, _rightMotorSpeed);

            foreach (LineSensor sensor in _lineSensors)
            {
                if(sensor.State)
                {
                    Debug.WriteLine("Sensor true");
                }
                
            }
            killOrthogonalVelocity(_leftWheel);
            killOrthogonalVelocity(_rightWheel);
            //Driving
            Transform tmp;
            _leftWheel.GetTransform(out tmp);
            Vector2 ldirection = tmp.q.GetYAxis() *_leftMotorSpeed;
            Transform tmp2;
            _rightWheel.GetTransform(out tmp2);
            Vector2 rdirection = tmp.q.GetYAxis() * _rightMotorSpeed;
            _leftWheel.ApplyForce(ldirection);
            _rightWheel.ApplyForce(rdirection);

        }

        public void DrawLine(SpriteBatch spriteBatch, Vector2 begin, Vector2 end, Color color, int width)
        {
            Rectangle r = new Rectangle((int)begin.X, (int)begin.Y, (int)(end - begin).Length()+width, width);
            Vector2 v = Vector2.Normalize(begin - end);
            float angle = (float)Math.Acos(Vector2.Dot(v, -Vector2.UnitX));
            if (begin.Y > end.Y) angle = MathHelper.TwoPi - angle;
            spriteBatch.Draw(_pixelTexture, r, null, color, angle, Vector2.Zero, SpriteEffects.None, 0);
        }

        private void setLinearVelocity(Body body, float spd)
        {
            Vector2 localPoint = new Vector2(0, 0);
            Vector2 velocity = body.GetLinearVelocityFromLocalPoint(localPoint);

            Transform tmp;
            body.GetTransform(out tmp);
            Vector2 sidewaysAxis = tmp.q.GetYAxis()*spd;
            body.LinearVelocity = sidewaysAxis;
        }
        private void killOrthogonalVelocity(Body body)
        {
            Vector2 localPoint = new Vector2(0, 0);
            Vector2 velocity = body.GetLinearVelocityFromLocalPoint(localPoint);

            Transform tmp;
            body.GetTransform(out tmp);
            Vector2 sidewaysAxis = tmp.q.GetYAxis();

            sidewaysAxis = Vector2.Multiply(sidewaysAxis, Vector2.Dot(velocity, sidewaysAxis));
            //float speed = sidewaysAxis.Length();
            
            /*if(speed != 0)
            {
                
                body.LinearDamping = 1 / sidewaysAxis.Length();
            }*/
            

            body.LinearVelocity = sidewaysAxis;
        }

        public override void Draw(GameTime gameTime, SpriteBatch batch)
        {
            //batch.Draw(_bodyTexture, _robotCenter, new Rectangle(0, 0, 100, 100), Color.White,
            //    (float)_rotation, new Vector2(50,50), 1.0f, SpriteEffects.None, 0f);
            Vector2 chassisPosition = ConvertUnits.ToDisplayUnits(_chassis.Position);
            
            Vector2 leftWheelPosition = ConvertUnits.ToDisplayUnits(_leftWheel.Position);
            Vector2 rightWheelPosition = ConvertUnits.ToDisplayUnits(_rightWheel.Position);

            batch.Draw(_chassisTexture, chassisPosition, null, Color.White,
                _chassis.Rotation, _chassisOrigin, 1.0f, SpriteEffects.None, 0f);
            batch.Draw(_wheelTexture, leftWheelPosition, null, Color.White,
                _leftWheel.Rotation, _wheelOrigin, 1.0f, SpriteEffects.None, 0f);
            batch.Draw(_wheelTexture, rightWheelPosition, null, Color.White,
                _rightWheel.Rotation, _wheelOrigin, 1.0f, SpriteEffects.None, 0f);

            

            Vector2 localPoint = new Vector2(0, 0);
            Vector2 velocity = _leftWheel.GetLinearVelocityFromLocalPoint(localPoint);
            DrawLine(batch, leftWheelPosition + localPoint, leftWheelPosition + ConvertUnits.ToDisplayUnits(velocity), Color.Green, 3);

            localPoint = new Vector2(0, 0);
            velocity = _rightWheel.GetLinearVelocityFromLocalPoint(localPoint);
            DrawLine(batch, rightWheelPosition + localPoint, rightWheelPosition + ConvertUnits.ToDisplayUnits(velocity), Color.Green, 3);

            //Transform tmp;
            //_leftWheel.GetTransform(out tmp);
            //Vector2 sidewaysAxis = tmp.q.GetXAxis();
            //sidewaysAxis = Vector2.Multiply(sidewaysAxis, Vector2.Dot(velocity, sidewaysAxis));
            //DrawLine(batch, leftWheelPosition + localPoint, leftWheelPosition + ConvertUnits.ToDisplayUnits(sidewaysAxis), Color.Green, 3);

        }

        

        public Vector2 Position
        {
            get { return _chassis.Position; }
            set { return; }  
        }


    }
}

